package com.hitqz.robot.biz.flow.component.agv;

import com.alibaba.fastjson.JSONObject;
import com.baomidou.mybatisplus.core.toolkit.Wrappers;
import com.hitqz.robot.api.business.model.dto.FlowContextDto;
import com.hitqz.robot.api.business.model.dto.FlowPatrolPreAndEndTicketsDto;
import com.hitqz.robot.api.common.entity.Robot;
import com.hitqz.robot.api.common.entity.RobotMapPos;
import com.hitqz.robot.api.common.entity.TicketsTask;
import com.hitqz.robot.biz.business.dispatch.DispatchService;
import com.hitqz.robot.biz.flow.config.FlowInterruptException;
import com.hitqz.robot.biz.flow.util.LiteFlowUtil;
import com.hitqz.robot.biz.service.RobotMapPosService;
import com.hitqz.robot.biz.service.RobotService;
import com.hitqz.robot.driver.model.IRobotPosDto;
import com.hitqz.robot.driver.parent.RobotDriverService;
import com.yomahub.liteflow.annotation.LiteflowComponent;
import com.yomahub.liteflow.core.NodeComponent;
import lombok.AllArgsConstructor;
import lombok.extern.slf4j.Slf4j;

import java.util.Optional;
import java.util.concurrent.TimeUnit;

/**
 * @author xupkun
 * @date 2024/11/5
 */
@Slf4j
@LiteflowComponent(value = "GoTargetPreSourcePosition", name = "[轮对/轮轴]前置站点-起点")
@AllArgsConstructor
public class GoTargetPreSourcePosition extends NodeComponent {

    private final RobotDriverService robotDriverService;

    private final RobotMapPosService robotMapPosService;

    private final RobotService robotService;

    private final DispatchService dispatchService;

    @Override
    public void process() throws Exception {
        LiteFlowUtil.checkInterruptFlag();
        FlowContextDto dto =  this.getRequestData();
        TicketsTask task = dto.getTicketsTask();
        log.info("任务数据 {}", task.getParseOrderParams());
        Robot robot = robotService.getDefaultRobot();
        FlowPatrolPreAndEndTicketsDto ticketsDto = JSONObject.parseObject(dto.getTicketsTask()
                .getParseOrderParams(), FlowPatrolPreAndEndTicketsDto.class);
        String sourceName = Optional.ofNullable(ticketsDto.getModalData().get("sourcePosName"))
                .orElse(ticketsDto.getModalData().get("sourcePosCode")).toString();
        String targetPosName = Optional.ofNullable(ticketsDto.getModalData().get("targetPosName"))
                .orElse(ticketsDto.getModalData().get("targetPosCode")).toString();

        //String auto = ticketsDto.getModalData().get("auto")== null ? "0":
        //        String.valueOf(ticketsDto.getModalData().get("auto"));
        //if (auto.equals("0")) {
        //    log.info("半自动任务:");
        //    return ;
        //}
        //IRobotPosDto posDto  = robotDriverService.getWebPos();
        //if (posDto.getFull()) {
        //    log.info("带载不去起点前置点");
        //    return;
        //}
        RobotMapPos robotMapPos = robotMapPosService.getOne(Wrappers.<RobotMapPos>lambdaQuery()
                .eq(RobotMapPos::getName, sourceName)
                .eq(RobotMapPos::getMapCode, robot.getMapCode()));
        if ((sourceName.equals("91101") || sourceName.equals("91102") || sourceName.equals("911005")|| sourceName.equals("911006"))
                && robotMapPos == null ) {
            String reName = sourceName + "-" + ticketsDto.getModalData().get("srcnum");
            log.info("新/旧轴上线区--起点" + reName);
            robotMapPos = robotMapPosService.getOne(Wrappers.<RobotMapPos>lambdaQuery()
                    .eq(RobotMapPos::getName, reName)
                    .eq(RobotMapPos::getMapCode, robot.getMapCode()));
        }
        if (robotMapPos == null) {
            robotMapPos =  robotMapPosService.getOne(Wrappers.<RobotMapPos>lambdaQuery()
                    .eq(RobotMapPos::getCode, sourceName)
                    .eq(RobotMapPos::getMapCode, robot.getMapCode()));
        }
        RobotMapPos prePos = robotMapPosService.getOne(Wrappers.<RobotMapPos>lambdaQuery()
                .like(RobotMapPos::getPreCode, robotMapPos.getCode())
                .eq(RobotMapPos::getMapCode, robot.getMapCode()).last(" limit 1"));
        if (prePos == null) {
            log.info("找不到前置点");
            return ;
        }
        log.info("前往起始目标点{}", prePos.getCode());
        if (prePos.getName().contains("911017")) {
            log.info("缓存架前置点-跳过");
            return ;
        }
        boolean isSuccess = dispatchService.go2TargetPosition(prePos, false);

        TimeUnit.SECONDS.sleep(3);
        IRobotPosDto iRobotPosDto = robotDriverService.getWebPos();
        while (true) {
            try {
                // 默认都是激光点
                iRobotPosDto = robotDriverService.getWebPos();
                if (iRobotPosDto != null &&
                        iRobotPosDto.getCurrentStation() != null
                        && prePos.getCode().equals(iRobotPosDto.getCurrentStation())
                        && iRobotPosDto.isReached()) {
                    log.info("机器人到达位置");
                    break;
                }
                LiteFlowUtil.checkInterruptFlag();
                TimeUnit.SECONDS.sleep(1);
            } catch (FlowInterruptException e) {
                break;
            } catch (NullPointerException e) {

            }

        }
        log.info("起始点机器人到达位置了");
        //log.info("pre" + JSONObject.toJSONString(iRobotPosDto));
        //TimeUnit.SECONDS.sleep(2);
        iRobotPosDto = robotDriverService.getWebPos();
        log.info("到达点位激光坐标 {}---- x: {}, y: {}, yaw {}", iRobotPosDto.getCurrentStation(),
                iRobotPosDto.getX(), iRobotPosDto.getY(), iRobotPosDto.getAngle()
        );
    }
}
